#include <Arduino.h>
#include "Seeed_vl53l0x.h"
#include "singleKalman.h"

Seeed_vl53l0x VL53L0X;
const int echo=19;
const int trig=18;
float sonic_speed=0.34;

long duration;
float timeDU;
float distanceMM;

int laser_distance;
double in_laser_distance;
double out_laser_distance;

void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);
  VL53L0X_Error Status = VL53L0X_ERROR_NONE;
  Status=VL53L0X.VL53L0X_common_init();
  if(VL53L0X_ERROR_NONE!=Status)
	{
		Serial.println("start vl53l0x mesurement failed!");
		VL53L0X.print_pal_error(Status);
		while(1);
	}

	VL53L0X.VL53L0X_high_speed_ranging_init();
	
	if(VL53L0X_ERROR_NONE!=Status)
	{
		Serial.println("start vl53l0x mesurement failed!");
		VL53L0X.print_pal_error(Status);
		while(1);
	}
  pinMode(echo,INPUT);
  pinMode(trig,OUTPUT);
  delay(500);
}

void loop() {
  // put your main code here, to run repeatedly:
  VL53L0X_RangingMeasurementData_t RangingMeasurementData;
	VL53L0X_Error Status = VL53L0X_ERROR_NONE;

  memset(&RangingMeasurementData,0,sizeof(VL53L0X_RangingMeasurementData_t));
	Status=VL53L0X.PerformSingleRangingMeasurement(&RangingMeasurementData);

  if (VL53L0X_ERROR_NONE==Status)
  {
    laser_distance=RangingMeasurementData.RangeMilliMeter;
  }
  in_laser_distance=(double)laser_distance;
  out_laser_distance=singleKalman(in_laser_distance);

  digitalWrite(trig,LOW);
  delayMicroseconds(2);
  digitalWrite(trig,HIGH);
  delayMicroseconds(10);
  digitalWrite(trig,LOW);
  //发射超声波信号
  duration=pulseIn(echo,HIGH);
  timeDU=(float)duration;
  distanceMM=timeDU*sonic_speed/2.0;
  // Serial.print("distance=");
  // Serial.print(distanceMM);
  // Serial.println("mm");
  delay(500);
  if(Serial.available()>0){
    char revicing=Serial.read();
    if (revicing=='t')
    {
      Serial.print("sonic distance=");
      Serial.print(distanceMM);
      Serial.println("mm");
      Serial.print("laser distance=");
      Serial.print(out_laser_distance);
      Serial.println("mm");
    }
    
  }
  duration=0;
  distanceMM=0.0;
  laser_distance=0;
  timeDU=0.0;
}

